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CHAPTEK 1 


Introduction 


A robot manipulator is a computer controlled mechanical arm which C5 
perform various tasks. These are gaining wide acceptance particularly i 
industrial automation like automated manufacturing, etc With rapid expansion in th 
field of robotic manufacturing systems, the problem of control of robot ar 
motion has become the subject of considerable research interest in the recer 
years. 

The robot control problem is to determine the joint torques that produce ■ 
desired motion of the joints. Most of the control schemes presently available an 
based on independent joint control scheme, in which each joint is independent!! 
controlled by simple PID type of control law with predefined constant gains. Thest 
control schemes may be adequate for simple pick and place tasks. However, many ol 
the modern day robots are required to operate under varying circumstances, suci 
as loading and with high speeds. Hence, the existing control schemes are severel!, 
inadequate. 


1 .1 Review of Literature : 

During the last decade, with the development of Adaptive Control theory, 
many authors made significant contributions to the field of control of robots by 
formulating adaptive laws and demonstrating their usefulness through simulations 
and experiments. 

One of the early contributions to the field of adaptive control of robot 
manipulators came from Horwitz and Tomizuka Ell. Their scheme assumed that plant 
inertia is time invariant and assumed gravity, friction torques to be known With 




these assumptions, they formulated each joint as a second order Single inp 
Single Output (SISO) system. Then for each degree of freedom they used i 
controller with an additional feedforward term to compensate for the friction ai 
gravity forces But with this formulation this system stability can not be assun 
under parameter variations. 

Another approach to the manipulator control problem can be classified i 
global linearization of the robot dynamics. Arimito and Takegaki L21 obtains 
adaptive control algorithms based on local parameter optimization for a robt 
described by a linear time varying model derived from the linearization around tt 
desired trajectory. This method assures stability of the error system. Howevei 
they assumed low speed of motion and neglected centrifugal and Coriolis force 
The simulation results with a nonlinear robot model produced tracking errors wit 
a maximum of i cm when the trajectory is a straight line with a maximum speed d 
0.1 m/s. 

Many of the later works in this area applied adaptive control theor 
developed for linear time invariant systems by making approximations about th 
nonlinear model. The highly coupled nonlinear equations of the robot model an 
linearized about the planned trajectory to obtain a perturbed system. Th( 
adaptive control law is based on the linearized perturbed equations about thi 
reference trajectory which takes into account all the interacting forces betweei 
the joints. Lee and Chung C33 used recursive least squares to identify th( 

I 

perturbed system and used one step ahead optimal control law to stabilize th« 
perturbed system. But when the perturbations are large such as load changes this 
control scheme may give large errors. I 

M.H.Liu [43 developed a control scheme using generalized least squares 
self -tuning controller with pole assignment. For each joint an independent control 
scheme is proposed using linearized dynamic equations. Various interactions 
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between the joints are neglected which result in poor performance at high speeds 
and also the loading effects are not considered. The simulation results show that 
the trajectory deviation is large in the begining of the trajectory due to improper- 
selection of the initial values. However, initial values play a very significant role 
in the parameter convergence of such schemes. This has been dealt with some 
detail by Waknis [53 and Ghosh et al [63. They showed that linearized models in 
general do not guarantee a minimum phase behaviour, and choosing of control 
parameters by trial and error may lead to severe divergence . 

Many control algorithms have also been developed which assume the full 
nonlinear manipulator model in the compensation These results belong to one of 
the two general categories : ” adaptit^e high-gain cancellation of nonlinearities " 
and " adaptii/e computed torque ". Of the first type, notable contributions include 
Balestrino [73 and Oshima et al [83. These adaptive schemes are similar in that 
they utilize tracking error feedback (position and rate errors) with an adaptive 
gain that grows until it is large enough to cancel the nonlinearities. An 
unanswered question in these schemes is how to select certain gains and 
parameters within the adaptation loop which depend upon the closed loop plant. 
Another major drawback with many of these schemes is that the control may chatter 
and excite the unmodelled dynamics ; and the robustness to these effects has not 
been established. 

The second category of the existing adaptive controller designs with full 
nonlinear manipulator plant model utilize the computed torque control law with 
some additional terms to provide for stability Basically, the computed torque 
technique uses the full dynamic compensation with feedforward and feedback 
components. The feedforward control components compensate for the interaction 
forces among various joints and the feedback components compute the necessary 
correction torques required to compensate for any deviation from the desired 
trajectory. Contributions in this area include Craig, Hsu and Sastry [93, Middelton 
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and Goodwin CiQ3, Slotine and Lee Cli3, [123 and AJ.Goldenberg etal C 133. In C113 
load and friction parameters are estimated as a part of a procedure whose aim is 
to force the error between the desired and actual joint coordinates to zero. This 
paper's main contribution is a theortical result. The proof of stability of the 
proposed globally convergent procedure is given. The robustness of these 
adaptive computed torque schemes with respect to plant uncertainties such as 
unmodeled dynamics, parameter variations has not been analyzed. 

All the above control schemes discussed assume noise free feedback 
measurements ( of positions and velocities of joints ) and no process ( robot 
model ) noise. But none of these requirements are fulfilled for a typical robot 
manipulator due to the relatively low resolution of the sensors emploCfed. Blauer 
and Belanger [143 attempted to solve this problem by using the Extended Kalman 
Filter theory. They expressed the noisy on-line state and force measurements as 
functions of stale and surface parameters. The Extended Kalman Filter is then 
used to give the optimal estimates of the states. Their simulation results show 
good convergence property and relative insenstiveness to increasing noise 
intensities. But they assumed that the robot dynamics is precisely known . 
Majee [153 solved this problem by estimating the robot dynamics using the 
tracking error information. 

Thus a large volume of literature available dealing with this subject vary 
on the basis of concept or implementation. Most of the methods made their 
control scheme robust by utilizing various assumed dynamic and kinematic models 
These models are typically inexact containing uncertainity in parameters. 
Moreover, most of the methods discussed above used adaptation as a means to 
simplify or otherwise to avoid performing complex computations in the control 
loop. As computing power becomes more readily available with the advent of fast 


microprocessors this motivation diminishes. 
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1.2 Scope of Work ; 

The objectives considered here for the control of the robot manipulators 

are 

i) The control scheme should be insensitive to noisy 
measurements of the system states. 

ii) The deviations from the desired trajectory i.a., the tracking 
errors should be small and convergence should be fast. 

iii) The control scheme should be robust enough to reject 
uncertainity in dynamics and its performance should be same 
at all operating conditions. 

Keeping in mind all the above objectives a control scheme based on Linear 
Quadratic Regulator theory is proposed. The basic control scheme consists of an 
identifier based on Recursive Least Squares procedure to identify the robot 
dynamics, a feedforward term computed from the identified dynamics to compensate 
for the various interaction forces among the joints and a feedback term 
computed from the identified dynamics to compensate for the model variations and 
parametric uncertainities. The feedback term is based on optimal control law 
derived from the linearized perturbation equations. This is coupled with the state 
estimator based on Kalman Filter theory to filter out any noise corrupting the 
feedback system. The importance of the identifier in the control scheme is 
demonstrated by comparing the performance of this scheme with the scheme 
without identifier in the control loop. Also, the performance of these two control 
schemes with two different state estimators. Extended Kalman Filter and Constant 


Gain Extended Kalman Filter is evaluated. 



& 


1 .3 Thesis Layout 

In ChapterZ the different control blocks which, when integrated 
constitute the full control scheme are introduced. These consist of a controller 
based on Linear Quadratic Control theory, and two state estimators based on 
Kalman Filter theory to filter out any noise corrupting the feedback 
measurements. The nonlinear continuous time model of the two degree of freedom 
planar manipulator model and the state trajectories used for simulation and 
analysis purposes are also included. 

Chapter 3 discusses one of the two control schemes presented in this thesis. 
This control scheme consists of feedforward term to compensate for the various : 
interaction forces among the joints, which can be computed off-line, and a feedback 
component based on LQ theory to compensate for the model variations and 
parametric uncertainites The noisy feedback measurements are filtered using the 
estimation schemes mentioned in chapterZ The performance of the controller in 
conjunction with each of the estimators is studied. Finally, the simulation results 
referred to the simulated robot model are presented. 

Chapter 4 provides the concept of Adaptive Linear Quadratic Control. Here 
the robot dynamic parameters such as load, masses of links etc. are estimated on 
line using standard recursive least squares technique and the feedforward term is 
calculated from these identified dynamic parameters. The performance of this 
adaptive controller in conjunction with each of the state estimators is studied 
using the simulated robot model. The simulation results and comparative 
performance evaluation of the two controllers are presented . 

In Chapter 5, two control schemes based on Computed Torque Control 
theory, which use full nonlinear feedback compensation are discussed. In the first 
scheme C153 the parameter perturbation effects are compensated by using sliding 
mode control. In the second scheme, which is essentially a modification of [ill, 
these effects are compesated by adapting the dynamic parameters as a part of the 

s 
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procedure to make closed loop system asymptotically stable. The results of the 
simulation experiments carried on the two degree of freedom planar manipulator 
are also included. 

In chapter 6 the performance of all the control schemes discussed in th 
this thesis are evaluated and the thesis is concluded with the summary of ideas 


which have been presented. 



CHAPTER 2 


Basic Control Blocks 


This Chapter deals with the various basic ideas which will be integrated 
later to derive the complete robot control scheme. The first of these ideas deals 
with the mathematical formulation of the dynamic equations of the robot arm motion 
which are useful for computer simulation of robot motion and in design of suitable 
control equations. These equations are derived for a two degree of freedom planar 
manipulator. This discussion is followed by a trajectory generation scheme through 
which the cartesian trajectory of the two degree of freedom planar manipulator is 
converted into its equivalent joint coordinates. The state space representation of 
the dynamic equations is derived which facilitates the control law 
formulation. Next, two global state estimation schemes to filter out the noisy state 
measurements for state feedback are discussed. The details of the linearized 
perturbation equations derived and the control scheme based on Linear Quadratic 
Regulator theory are also included . 


2.1 Dynamics of Robot Manipulator 


The closed form dynamic equations describing the robot arm motion are 

given in the vector-matrix notation C163 as 

u = D<a.)g + H(a,9> + gfa_) (2.1) 

where 9, & g, u are n dimensional joint position, velocity, acceleration and joint 

nxn 

torque vectors, respectively; D(^ G R V 9 is the manipulator moment of inertia 

nxi 

matrix; HCg^g) € R V a g represents the centrifugal and Coriolis forces acting 

nxl 

on the joints; g(a> € R V 9 represents the gravitational loading on the joints. 


In the derivation of eq. <2.1), the actuator dynamics, the terms due to 




ri ana ettects due to torque saturation are neglected. Even with these 

simplifications, it is seen that the underlying modes of eq. (2.1) are nonlinear and 
strongly coupled. The dynamic model, eq. (2.1) forms the starting point for all the 
control efforts described in the following pages 


2.2 Robot - Simulation : 

A two-degree of freedom planar manipulator is chosen for the experiments 
performed in this work. The manipulator configuration is shown in fig. (2.1) 



Fig. 2.1 The Model Robot 


For this two- degree of freedom manipulator, the total number of joints, n is 
equal to 2. Under this condition, the torque and position vectors are given by 


“1 

“2 


' a 


di 


and the D, H and g matrices are given by 


DCg^ 


Dii D«g 


1 

X 

t 


3 ii<a) 


. H ( g ,g) = 


and g( g > = 


Dgi D22 


Hati 

M mi 


321(a) 


( 22 ) 


( 2.3 ) 





iO 


These matrices are then written in terms of the robot link parameters 

(defined in Table (2.1) ) as 
2 2 2 

Dij. = mj.lCi + mgC li +IC2 + 2 Ixlc^cosCos) + I2 +I2 

2 

Di 2 = inj^llC2<=»3S(C)2)+n^lC2 + I2 
^21 = Di2 

2 

D 22 = IB2lCjit 

2 

Hii = -inj^ilC2Sin(d2 >02-2 m2lC2liSin(q2}qi'q2 ( 2. 4 ) 

. 2 

H 2 J. = l!^l.ilC2 sin(ci2)Q2 

9ii = mtlci3 cas(qi}-i-in29 ( lc2Cos(qi+q2>+liCos(qi> > 

321 = fn2lc2 3 cosCqi+qa) 

Table 2.1 


link 

Mass 

Length of 

Distance of cjn of link i 

Ina'tia of Link i 



the link i 

f rcHii i-th coordinate 



( Kg) 

( m. ) 

frane( mi 

( m ) 

1 

ffli = 15 

Ml 

ICj^ = 05 

li = 3.9765 

2 

■ 

1 

l2 = 0.8 

IC 2 = 02 : 

I 2 = 0£338e 


The robot model is then simulated in disital computer by solving the seoond 
order non-linear differential equation ( 2.1 ). The solution here lies in finding 
the joint position, velocity and acceleration vectors along a specified trajectory. 
To slove for these quantities the torque vector along this trajectory is given as 
an input. Many standard numerical methods like Runge-Kutta, etc. can be used to 
solve this problem. The method adopted in this thesis is similar to the one 
described in E153, E173 where a detailed implementation algorithm is also given. 
















2.2.1 Trajectory Generation : 

The nominal trajectory for the robot, i.e., the reference trajectory through 
which the robot must travel is specified in terms of the desired end-effector 
velocity, acceleration and the end effector displacement with respect to world 
coordinates. The problem of trajectory generation is to convert these values in 
terms of joint positions, velocities and accelerations at each instant of time. This 
is referred to as the inverse kinematics problem. 

The two reference (nominal) trajectories considered for simulation purposes 
are discussed below. 

TRAJECTC3RY 1 : It is assumed that the end-effector travels in a semi-circular 

path with radius r. The center of the circular path is at a distance 'a' from the 
base of the model robot. This trajectory is shown in fig (2.2). Let the rotation of 
the tip be in counter-clockwise direction. 



Fig. 2.2. Nominal Trajectory i 
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Then the equation of the circle is given by, 

2 2 2 

(x-a) + y = r < 2.5 ) 

Considering the angle of rotation to be $ radians, the cartesian points can be 
determined from the following relationship. 

X = a+r cos(0> 

y= r sin(6) C 2.6 ) 

The trajectory is to be followed in the given fashion. 

i) The robot is at rest at a point Xq. Initial joint velocity and 
acceleration of each joint at this point are zero. 

ii) The arm then starts frcsm rest with the end effector moving with a 

2 

constant acceleration of it rad. /sec. , until its velocity becomes 
n/2 rad /sec. 

iii From this point, it moves with constant velocity of it rad / sec. 
until 9 becomes it/8 rad. 

2 

iv) Then it decelerates with constant deceleration of it rad./ sec until 
9 becomes it/2 rad. At this point the acceleration and velocity are 
made zero such that the arm is at rest. The whole process has to be 
finished in 2 sec. 

TFJAJECTORY 2: For trajectory 2 it is assumed that the end effector travels in an- 
elliptical path with radii and r 2 - The center of the elliptical path is at a 
distance 'a' from the base of the model robot. Fig (2.3) shows the end effector 
trajectory. Let the rotation of the tip be in counter-clockwise direction. Assuming 
the angle of rotation to be 9 radians, the cartesian points can be determined from 
the following relationship. 

X = a-HTj^ cos(9> 


y= r 2 sinC9) 


( 2. 7 ) 
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X 



The trajectory is to be -followed in the given -fashion. 

i) The robot is at rest at a point Xq. Initial joint velocity and 
acceleration of each joint at this point are zero. 

ii) The arm then starts from rest with the end effector moving with a 

2 

constant acceleration of rad ./sec. , until its velocity 

becomes 2 it/ 3 rad. /sec. 

iii) From this point, it moves with constant velocity of 2 tc/ 3 rad/ sec. 

until & becomes 3it/2 rad. 

2 

iv) Then it decelerates with constant deceleration of 411/3 radysec 

until & becomes 2it rad. At this point the acceleration and 
velocity are made zero such that the arm is at rest. The whole 
process has to be finished in 3 sec. ; 

After generating the two trajectories the next step is to calculate the 
manipulator joint positions, velocities and accelerations from this information, i 
For constant acceleration or deceleration the following relation holds, i 

d®© / dt® * 


C 


< 2S y 




Where C is a constant whose magnitude is positive if it is acceleration and 
negative if it is deceleration. 

Similarly for constant velocity region of trajectory the following relation holds, 
de / dt = C ( 2.9 ) 

The solution of the differential eqns. ( 2.8 ) and ( 2.9 ) can be obtained by using 
standard numerical integration methods such as Runge-Kutta etc. For the present 
purpose. Trapezoidal method is used to solve for S at every discrete instant k. 
The values of Sflc) V k thus computed are stored and subsequently used for 
finding the reference trajectory using inverse kinematic relationships given 
below. 

cas(q2> = <x® + y® -if -1^ ) / 2 I 1 I 2 ( 2.10 ) 

qj = tan"*’( y/x) - tan"^ € t I 2 5in<q2>l / C +1 2 Cos< q 2 ) 3 } ( 2.11 ) 

The vector g(k>, containing the joint positions, thus obtained is differentiated to 
get gCk) and g(k). 

The overall algorithm for finding joint trajectory for the circular and elliptical 
end effector trajectories can be found in C 28 3. 

Fig ( 2.4 ) and Fig ( 2.5 ) show the two nominal joint trajectories ie., the 
joint position and velocities for the semi-circular trajectory and elliptical 
trajectory, respectively. 

2.3 State Space - Formulation : 

The manipulator dynamic equation (2.1) can be rewritten in order to 
facilitate the state-space formulation as 

g = D‘^(gp Cu--H(a.g>-3ta>> ( 2 . 12 ) 

As D is a positive definite matrix representing manipulator inertia', D ( g ) 
always exists. 

For the trajectory control of a robot manipulator the joint position and 
velocity vectors are usually feedback through a feedback mechanism. Thus the 



joint 1 voiocHy Joint 1 position 






Rg. 2.4 Nominal Trojector^ 1 



iJokrt 1 posiUori 






Rg. 2.5 Nominal Trajectory 2 
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most obvious choice for as state vector is xT = C gTft) gT (t) 3 = [ xT(t) g m) 3 

where x (t> € fl . Eq. ( 2 . 1 ) then can be rewritten in terms of the state vector as, 

a g (t) 

D ^(gp C y-H(g,g)_g^ 3 

which gives the state-space representation of the form 
= f Qe, u(t), t ) . 

Eqns. ( 2.13 ) and (2.14 > express the nonlinear manipulator dynamics in terms of 
state-space representation. 

From this state-space representation a suitable controller which will 
stabilize the closed loop system can be designed by using any of the standard 
control techniques. But almost all the control schemes assume perfect state 
information for feedback, i.e., the measurements of the joint positions and 
velocities are noise free. In a real life situation, the measurements are often 
corrupted by noise. Thus for good tracking, state estimation technique is 
necessary to disambiguate the feedback measurements. The following section deals 
with this problem. 

2.4 State Estimation Problem : 

The problem is now to estimate the state of a non-linear time varying 
system through discrete and noisy observations of the system state. Many authors 
have worked on the development of state estimators for non-linear systems. A 
survey of the currently available recursive state estimation schemes applicable to 
a br oad class of non-linear systems can be found in ti83. For the present purpose, 
an Extended Kalman FilterC EKF) and a Constant Gain Extended Kalman Filter (CGEKF) 



are studied. 
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2.4 .1 Extended Kalman Filter 

The Extended Kalman Filter, which is used for nonlinear filtering is a 
natural extension of the Kalman Filter for linear systems. The signal model of the 
robot manipulator is thus given in terms of eq. ( 2.14 ) as, 

i = £( 3e, y,t ) + u(t) ( 2. 15 ) 

ijCk) = h<x(k), u<k), k ) + i^tk) ( 2. 16 ) 

Eq. <2.15) is similar to eq. ( 2.14) except for an additional term oKt), which 
represents the process noise . 

It is assumed that the output measurements are available at discrete 

instants Eq. (2.16) represents the output equation, where is a vector 

nxn 

containing measurement noises, and h € 1% is a non-linear function. It is 

assumed that ~ 0, R ), is white, zero mean and uncorrelated such that 

T 

E[£(k)i/ (j) 3 = R S<k,j) , where S<k, j > is Kronecker delta function defined by 
S(k , j) = 1 for k =3 

S(k , 3> = 0 for k#3 ( 2.17 ) 

Theoretically the process noise igCt) should be absent if the chosen state 
model accurately represents the robot dynamics. But it is introduced in the state 
model for the filter tuning purpose. It is assumed that the process noise 
ttfCt) ~ 0, Q) is also white zero mean Gaussian noise and is uncorrelated with 

ijtk). To solve the state equations <2.15 ) and <2.16 ) an initial state x 0) has to be 
defined. However, as the signal model is stochastic, this initial state is also 
defined as a Gaussian Vector and with added assumption that Et x^O m (0> 3 = 0, 
iKO) ~ (p3),P0)) , where Q€ and RD) € R^hXZn covariance 

matrices of io<t) and x0); p39 is E( icCO) > . 

Eqns. <2.15 ) and <2.13 ) can be rewritten to obtain 


a 


a 

a 


D"^(g) Cu - H<g , g) + g<a ) 


+ u(t) 


< 2.18 ) 
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It is to be noted that in the signal model, the state equation is given in 
continuous time while the measurements are taken at discrete instatants. Also note 
that the measurements are taken for position and velocity of each joint. Then the 
measurement equation becomes 


tj(k} 


^ 0 
0 In 


gOe) 


+ u<3d 


< 2.19 > 


The step by step algorithm for continuous time EKF with discrete 
measurements is given below Ci93. 

Algorithm : 

InmA data : XCO) . P<0) and m>. 

STEP i : Measurement update at each instant K is computed as, 

Kalman Gain : L<K) = P<k/k-i) t Pflc/k-D + R 3 ( 2120 ) 

Error cwariance : P(k/k) = C I -LOc) 3 PCk/k-13 ( 2J21 ) 

Estimate x = xOc/k-D + L(k) C yOe) - »k/k-D 3 ( 222 ) 


STEP 2 : Find F^t) for x = xCk/k) that is evaluate 3£/^ . 

STEP 3 : Time update of equations are given by 

Estimate • £ = £( x , u , t ) -i- ^t> < 223 ) 

Error cwariance •- P = , t> P flc/k> + P '*^<k/k) , t ) + Q(t) i 224 ) 


Iritegrate * and P given in eqns. ( 2.23) and (2.24) using Runge-Kutta or any 
other numerical integration routine to find x ( k/fc+D and P<fc/k+D. 


For good convergence the filter is to be tuned properly. This means a 
proper selection of the covariance matrix, Q of pseudo-process noise wCt) added 
deliberately to the dynamic model. This is done to control the convergence 
property of the filter and to take care of model inaccuracy arising from various 
approximations and accumulated round-off errors. Q can be chopse® as positive 
definite diagonal matrix so that convergence of each joint position and velocity 


can be controlled individually. 



The covariance matrix FKt) propagated by the filter equations should always 
remain positive-definite. But during computer simulation Rt) may become non 
positive definite due to accumulated round off errors, which may result in the 
filter divergence. In case of severe divergence a square-root algorithm or an 
U-D algorithm might be required. However, for simpler cases by choosing proper 
value of Q this problem might be alleviated. 

2.4.2 Constant Gain Extended Kalman Filter : 

The EKF algorithm discussed in the previous section is computationally 
intensive and it significantly limits the scope of applications. The major portion 
of the computational burden results from the the calculations associated with the 
propagating Error Covariance Matrix P(k), which in turn is used for real time 
updating of the gam matrix L<k) on the filter residuals. When one considers the 
gross nature of approximations that are routinely made in modeling the stochastic 
disturbances effecting a system and to a lesser extent in modeling the interplay 
between the disturbances and system non-linearities, it seems somewhat surprising 
that so much computational effort should be devoted to careful propagation of the 
model's error covariane matrix. Based on these observations, MS. Safonov and M. 
Athans [203 developed a Constant Gain Extended Kalman Filter in which the gain 
matrix acting on the residuals is assumed to be constant. Interestingly, the CGEKF 

I 

has all the intrinsic robustness to modeling errors as EKF and it drastically 
reduces- the Computational burden. 

The Constant Gain Extended Kalman Filter is the filter given by equation 
(2.22 ) in which the gain matrix L(k> is a constant given by 
L = P E P + R 3 


here P is the solution of the Ricatti equation 


(225 ) 
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Mgorithm : 

STEP 1 : Estimate »Ck/k> = |Ck/k-D + L C yOe)- x (k/k-D 3 * ‘ < 226 > 

STEP 2 : Time i43date of ec^iation ( 225 > is 

^t> = f ( X , u , t > + ^t) 

INPUT : xCO) and L . 

The matrix L can be obtained from eq. (225) by choosing a proper value of 
the pseudo process noise covariance matrix Q. 

2.5 Linear Controller 

The choice of control law may depend upon many features including 
i) is the process open loop unstable ? 

, ii) is the measured output corrupted by noise? 

iii) are there are any constraints on available computing power? 

iv) is excessive actuator movement undesirable 

Therefore, the choice of control law is highly application dependent. Ultimately, a 
trade off must be made between inuDroved quality of control and controller 
simplicity. 

In the present work, a linear quadratic controller is used for the robot 
control This as the name implies, can only be used on a linear model However, as 
discussed earlier, the robot model is highly nonlinear and hence has to be 
linearized. This linearization is done around a nominal trajectory to obtain a 


perturbation model. This is discussed below. 
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2.5.1 Linear Perturbation Model ; 

A linearized perturbation model can be obtained by expanding eq. <2.14 ) 
about the nominal trajectory by using Taylor's series and neglecting the higher 
order terms. This is given by, 

= Vx f| d + Vu d 

= Mt> SxCt) + B(t) Su<t) ( 226 ) 

Where A<t) = Vx’fj d fj d jacobian matrices of f< xffc> , yCfc) > 

evaluated at (the desired state vector at time instant t > and u^t) ( the 

desired torque vector at time instant t ) respectively and 

Sx(t) = X (t) - X(j <t) ( 227 ) 

Syft) = u (t> - Ujj ft) ( 228 ) 

§C1SV 

Eq. (226) represents a sssstcwi order linear time varying system. The system 
parameters A(t) and Ht) vary very slowly with time and depend upon instantaneous 
manipulator position and velocity . But if the control update speed is increased, 
then the system can be viewed as a linear time invariant system so that the 
argument t can be dropped. The nominal torque vector u^j (t) can be calculated from 
an inverse dynamics algorithm like computationally efficient Newton-Euler 
formulation. The state vector »Et), comprising of the joint pas t in e ss and velocities 
can be assumed to be available for measurements. 

The computation of the system matrices A and B poses a serious problem. 
For a simple two link manipulator, some of the elements of A and B contain over 50 
components. So this increases the computational burden which is unavoidable. The 
next step is to determine a linear quadratic control law based on this 


perturbation model. 
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2.5.2 Linear Quadratic controller 

As a result of this formulation, the manipulator control problem is reduced 
to determining Su(t), which drives Sx(i) to zero at all times along the nominal 
trajectory. The overall controlled system is thus characterized by a feedforward 
component and a feedback component Given the planned trajectory set points (q^jCt), 
q^U, QfjfH ), the feedforward torque computes the corresponding nominal torques 
u^t) from N-E equations of motion. The feedback component computes the 
corresponding perturbation torques SyCt) which provides control effort to 
compensate for small deviations from the nominal trajectory. The main advantages 
of this formulation are two fold. First, it reduces the non-linear control problem 
to a linear control problem about a nominal trajectory; second, the computations of 
the nominal and perturbation torques can be performed separately and 
simultaneously. After obtaining the linearized perturbation equations the next step 
is to formulate the control law, .which is based on the following performance 
index. 

oo 

^ =‘ j r Sx (« + e SuCt) ) dt ( 2.29 ) 

0 

where r=r^2: 0 and e = e^>0 
The optimal control law is given by 

Suit) = - Sx ( 2.30 ) 

where T = T^> 0 is the solution of the algebraic Ricatti equation, 

A^T -I- T A + r - TB ©"^^T = 0 ( 2.31 ) 

The above formulation is based on the assumption that the state measurements 
are noise free. In the case of noisy measurements modified the performance index 


becomes 
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J = E 



(Sx* (t) r 5x (t) + su'tt) e stxt) > dt 


( 2.32 ) 


and the control law is given by 

Su(« = - fix <l) < 2.33 ) 

where Sx = x - x and x is the estimated state vector -from the noisy measurements 
o-f X obtained from either eq. (2.22 ) or eq. (2.25 ) 

The above control scheme differs from the other control schemes based on 
linear time invariant control schemes in the sense that it takes into account all 
the interactions among the various joints. 

The basic control blocks discussed here are used to design a control 
scheme which will statbilize the closed loop system in presence of model 
uncertainties and load variations in some optimum sense. This requires some 
modifications to be made in the state estimation to take into account the model 
uncertainties in the system dynamics and in the nominal torque calculation to take 
into account the load variations, which are dealt in the following chapters. 



CHAPTER 3 

Robust Controller 

With 

Nonadaptive Feedforward Compensation 


In this Chapter the various modules described in Chapter 2 are 
integrated with the necessary modifications required for the robot manipulators. 
The modification is mainly in the state estimator to take into account the fact 
that a good representation of ths system dynamics, which is required for the 
filter convergence is not available due to imprecisely known dynamic parameters. 
The performance of the control law based on LQ theory in conjunction with the 
modified estimator algorithm is analyzed by implementing the control scheme on 
the simulated robot. The simulation results are also presented. 

As mentioned in Chapter 2 the control scheme consists of a feedforward 
term to compensate for the nonlinearities and feedback term based on LQ theory 
to compensate the parameter perturbations and loading effects. The basic block 
diagram of the control scheme is shown in fig ( 3.1 ). The noisy measurements of 
joint positions and velocities are estimated by the modified EKF/CGEKF. The 
feedforward component of the control torque can be calculated from the Newton- 
Euler formulation and the feedback term based on LQ optimization principle is 
calculated from the linearized perturbation equations. It can be observed that the 
feedback and feedforward components of the control torque can be calculated 


simultaneously. 




Diaqram of tho Control Schomo 
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3.1 State Estimation with EKF : 

Consider the system given below, 

X = £ ( 3.1 ) 

where x € is a non-linear continuous function of x , u and t. In the case 

of the robot manipulators, neither the function £ is exactly known nor the 
exact feedback measurements of state x are available. The EKF is used to 
estimate the state vector x from the noisy observations. The model chosen for 
EKF given in eqn. (2.15) is modified as shown below. 

X = £ (2E» u»t ) + wCt) ( 312 > 

A 

where £ <x, u, t ) is the estimated dynamics model . 

In the theory of EKF there is no in built mechanism for dealing with the 

A 

model inaccuracy in £^ u,t ). Usually the torque/force vector is computed from 
a feedback control mechanism and an improper compensation by the controller may 
complicate the problem. In various cases this model inaccuracy may cause serious 
divergence problem when theoretical model behaviour does not agree with the 
actual behaviour. In mathematical sense, the divergence of the filter occurs when 
actual error correlation is not bounded as expected from predicted error 
covariance Pjj or in an apparent divergent problem remains within a bound 

which is greater than predicted by Pjj. S^, and Pj^ are defined as, 

\ = E I C _ X ( k/Tc-D 3 I i ( l^-i> 3^ } 

P,j = E I t ^ - 'x ( k/k-i ) 3 [ ^ - X < k/k-1 > 3^ } ( 3.3 > 

Jifl flt iml 

where 3 ^ plant stale output and 3 ^ is model state output. 3 c (k/k-D 

is the not updated slate at t = t^.. So the problem is now of two fold, 

* To prevent any possible divergence of the filter output. 

• To track the actual system parameter by a simple adaptation law. 


Finally, both the filter and tracking controller should work together as a 
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overall controller. 

a.2 state Estimation with CGEKFF : 

All the above arguments hold good even for the CGEKF since it is 
essentially an EKF in which the error covariance matrix is not propagated in 
order to reduce the comutation. 


3.3 Estimation of System Parameters ; 

Let a trajectory tracking error be defined as 
e = < X - Xjj ) ( 3.4 1 

e = £(e^y,t) { 3.5 1 

Let the model of eq. ( 3.2 ) be redefined as , 

X = f <x, + w(t) C3.6) 

where f ^ u,t) is the nonlinear function which is used. 

In order to take into account the errors due to perturbations in the plant 
parameters this is defined as [15 3, 

£ (x, = £ (x, u , t) - ^ 4 ( 3.7 ) 

JFt, 

where j3 is a positive definite matrix. The nonlinear functional f is the 

approximate dynamic functional available i.e., 

f Ujty = D“^<C 3 (} [ u - H<q,q> - g(q) 3 t 3.9 ) 

where 

D a D -h &D 

A = H + »^ { 3.1D ) 

g = g + Ag 

4D, AH and Ag are the uncertainties associated with D, H and ^ matrices, 
respectively. 

This formulation can be regarded as a joint state and parameter estimation 
problem in which the system parameters are estimated from the tracking error 



29 


information. For proper convergenoe of the control law, it is important that 0 be 
tuned properly. 0 can be partitioned as tl93 



r* 




o 

0 = 

o 

182 


where 0 ^, 02 ^ ^ It can be observed that 0^ is associated with the positional 
components of the state vector and 02 with the velocity components of the state 
vector. As the system dynamics has no direct effect on the position vector 0 ^ 
can be assumed to be diagonal matrix with diagonal elements nearly equal to one. 

The uncertainty matrices can be modeled as norm bounded perturbations, i.e , 
the norms (lADjl, llAHlI and IIAGII are bound by some upper limits. 02 then can be 
defined approximately in terms of these uncertainties as 

= [i - [ U - H(g ) - 3<a> ] 

Based on the limits on the uncertainties of the dynamic parameters, jlfti(| can now 
be calculated in a straightforward manner. 

3.4 Control Strategy : 

It is assumed that the measurement noise statistics are available and the 
nominal values of all the robot dynamic parameters are known. It should be noted 
the estimator described above does not play any role directly in the control 
scheme, but it affects the feedback compensation. The basic steps to be followed 


are given below. 



Algorithm 

Initialization : 

Initialize matrices Q, R, and RO) and initial state x (0). Find a 
suitable $ by following the arguments given by eq. (3.11 ) which 
produces maximum convergence. 

For CGEKF, choose L matrix by trial and error to give maximum 
convergence. 

Choose the r and © matrices of the controller. 

Step 1 : 

Tune the system parameter of the model x = f(x,u,t) by 
f{x,y,t) = f<x,u,t)- i5e 
Step 2 : 

Use the EKF or CGEKF algorithm to find the estimates of position 
and velocity using the system and measurement model as given 
here. 

X = ftx# u ,t ) + w(t) 
a(k) = h|,5^ + Pjj 

and giving output E x (t|j. ) 3^ = t g (^j) g ( ^ ) 3 
Step 3 : 

Find the nominal torque Ct) from 

= D<gj,,^)gjj + H + g(gc() 

Step 4 : 

Evaluate the jacobian matrices A(t) and B(t) given by eq. (2.26 ) and 
solve the Ricatti aquation (2.32 ). The perturbation torque is 
given by, Su <t) = - B^<t) T (t3 , where T is the solution of 

the Ricatti equation. 

Apply the control torque y (t) = (t) + S u <t). 
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3.5 Numerical Example : 

The above design is used to build a controller for the robot model 

described in chapter 2. Computer simulation is carried out to find the 

performance of the control logic when a load of 4 Kg. is attached to the second 

link The state vector for this model is given by , 

T T 

X = [ 02 ^ ^ 

Thus the state equation is, 


X = 







02 



9 ' a * y * t ) 

d2 


f 2< g , a » M « t ) 


+ w (t) 


( 3.12 ) 


The actual expressions for f and f 2 are given Appendix 2 
The jacobian matrices A(t) and B(t> are given by 


Aa) = 


0 

0 

1 

0 

0 

0 

0 

0 

3f jl^/3qj^ 

3f j^/3q2 

afj^/aq'i 


3f2/3q'l^ 

af2/3t^ 

afg/adi 

1 

af2/aq 


B(i) = 


0 0 

0 0 

3f j^/0iJ2 
3f2/9UjL 3f2/ 


3fi/3Ui 


j < g^t) , > 

The detailed expressions are given in Appendix B. 


( sijjCt) , g^t) , U(j(t) ) 


It is obvious that the performance of the above controller depends upon 
the cost-function parameters r and 6 . The F and 0 matrices can be chosen to be 
diagonal for independent tracking of the joint positions and velocities. The 
control weighting parameter 6 determines the relative importance which is to be 
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placed on the penalization of control by cost-function. It follows that a high 
value of © leads to less control variation and a highly damped output, while a low 
value of 8 leads to a lower variance of tracking error and smaller offset. It is 
also possible to choose dynamic weights in the solution of the optimal controller 
to allow error and control signals to be penalized differently in different ranges. 
Thus r and © are given by 
r = diag ( Tu , Taa > snd 8 = diag ( 

For the filter the measurement equation is given by eq. ( 3.6 ) As direct 
sensory measurements of both velocity and position are used, the matrix is 
assumed to be linear and equal to identity matrix. It is assumed that the 
measurement noise covariance matrix R is diagonal such that each measurement 
noise is uncorrelated with others . Then, 

R = diag t ^ ) 

To select x(0) a priori knowledge of initial positions and velocities are required. 
To start the algorithm x(0) is set to measured value of initial positions and 
velocities . Next step is to compute FKO > from E Cli x <0) - x <0) H So from the 

very choice of , P<D) becomes P(0) = R. To run the algorithm the jacobian 
matrix F( x ,t ) is required which is similar to the A(t) matrix given above. This 
is given by. 


F (X ,t) = 


a f ( X , u , t ) 

ax xOCiTe) 


O 

0 

8fi/aqi 


o 

af^/acb 


1 

0 

afi/aa'i 

af2/acij|^ 


o 

o 

afi/aci2 

af2/ad2 


( xOc/Tc) 


With the above formulation, the the control algorithm is applied to the simulated 
robot arm. Parametric variations are incorporated into basic dynamic parameters, 
i.e., the mass of a link, moment of inertia of a link and the center of mass. Table 
(3.i) gives the list of dynamic parameters used for generating the control logic. 
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The actual dynamic parameters are already presented in table ( 2 i) 

Table 3.1 


liriic 

Hass 

Laigth of 

Distance of cm of lirtc 1 

: 

Inw'tia of Link i 



the lirdc i 

•fpcMil i-th cxic^inaie 



( Kg) 

( m. ) 

framet m3 

( m > 

1 

mi = 14 

I 2 = 1.4 

ICi = 0.45 

li = 3.75 

2 

^ = B£ 

I 2 = 0.8 

Icz = 0.175 

Iz = 05 


The covariance matrix of the pseudo process noise vector tfCt) is chosen by 
trial and error method. Q is chosen to be diagonal with positive entries, whose 
values are adjusted to give the best convergence. 

Now, coming to the estimation of noisy feedback measurements, noise variance 
of the position transducers is assumed to be small where as the noise variance 
of the velocity transducers is assumed to be comparatively larger to reflect the 
relative inaccuracy of the velocity sensors due to their low resolution. The matrix 
L in the case of CGEKF is chosen by trial and error to give maximum covergence. 
Table (3.2) shows the design parameters of the filter and controller combination. 

The simulation runs for the control scheme are performed for the two 
different state trajectories discussed in Chapter 2 by employing the two state 
estimation schemes EKF and CGEKF. Table (3.3) shows the maximum tracking errors 
for the trajectories i and 2 with the two state estimators EKF and CGEKF in the 
control logic and table (3.4) shows the corresponding variances of the tracking 
errors. The maximum error refers to the maximum deviation from the desired slate 
trajectory. Fig (3.2), fig (3.3 ) show the tracking errors for the two nominal 
trajectories with EKF as the state ^timator in the control scheme and fig (3.4 ) 
and fig (3.5 ) show the corresponding plots with CGEKF as the state estimator. It 
can be observed from these figures that the control scheme with EKF outperforms 
the one with the CGEKF. This is quite expected as, in EKF the error covariance 


matrix is propagated whereas in the case of CGEKF, it is not. It can be found 
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that the tracking errors are somewhat large in all the cases, which can be 
attributed to the fact the actual dynamics of the system is perturbed and the 
nominal torque obtained form the the model of the system is not the nominal torque 
required by the manipulator under perturbed or loading conditions. This 
observation forms the starting point for the control scheme discussed in the next 
chapter. 


Table 32. 

Design ps^saisters of the tracking controller ; 

Q = diag fiOOO, 1000, 1000, 1000 > 

R = diag ( 1, 1 ) 

Design parameters of the EKF : 

R = diag( 0.001, 0.001, 0.05, 0.05 ) 

Q = diag( 0.0012 ,0.0012, 0.0005, 0.0005 ) 

P(0)= R 

iS = diagC 0.9 , 1.3, 2.1 , 3.1 ) 

Design parameters of the CGEKF ; 

R = diag< 0.001 , 0.001, 0.05 , 0.05 ) 


L = diag <0.01475, 0.01475, 0.15, 0.15 ) 
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Tatum 33 




Maximum traukino £rrar 

Maximum Traoldno trrar 



Mith 1 

EKF 

Kith O 

3E1CF 

nmm 

joint 

pocitinn 

veLocity 

position 




(racD 

<rad/s) 

(rad) 


Semi- 

1 

0.01383 

0.07818 

0.02134 

0.06121 

Circular 

2 

031921 

025876 

032587 

030799 

Elliptical 

i 

032266 

0.07677 

0.04545 

0.15464 


2 

031560 

0.19864 

0.02427 

0.41064 


TakOm 3.4 




Variance of Tracking Error 

Variance of tracking Errt^ 

Trajectory 


Mith 1 

EKF 

Mith O 

3E1CF 


joint 

position 

vmLociiu 

position 

vmlocitu 



(rad) 

(rad/s) 

(rad) 

(rad/s) 

Semi- 

1 

0.00036 

0.03163 

0.00176 

0.00389 

Circular 

2 

0.03026 

0.00321 

0.00052 

0.00544 

Elliptical 

1 

030038 

030108 

0.00559 

030185 


2 

0.00064 

0.00870 

0.00474 

0.CK3886 
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CHAPTER 4 


Control Scheme 
With 

Adaptive Feedforward Compensation 


In this Chapter an adpaptive control scheme for the robot manipulators is 
suggested. The method presented in this chapter is inspired from the previous 
control scheme discussed. Instead of using the nominal torque from a model which 
presumedly is not exact, the nominal torque is calculated from a model based on 
identified robot dynamics. A recursive identification scheme is used to identify 
the dynamic parameters. Thus both the loading effects and parameter perturbation 
effects are captured at once. As in the case of the previous scheme, the closed 
loop system consists of linearized dynamics obtained from perturbation equations 
and a LQ type of optimization is used. Finally, the petH^ormance the above control 
scheme in presence of measurement noise is studied by implementing this modified 
control scheme on the simulated two degree of freedom planar manipulator. 

4.1 Dynamic parameter Estimation ; 

The dynamic model used in computing the nominal torque is given by eq. (2.1) 
which can be computed recursively by using NE formulation. The nominal torque 
does not fully compensate for loading or parameter perturbation effects, since it 
is calculated from the known imperfect information about the dynamic parameters. 
In principle, the linear controller discussed in previous Chapter would compensate 
for these disturbances if the perturbations are small and in the vicinity of the 
nominal values. Below an approach is presented where the nominal torque is 
computed and compensated based on identified dynamic parameters using recursive 
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least squares technique 

Knowledge of manipulator loads and links (mass, center of mass and moment 
of inertia) is potentially important for the precise control of movement Robot 
manipulators are designed aoDording to precise kinematic specifications, but the 
link parameters are incidental attributes of design. The NE equations describing 
the robot dynamics can be recast in such a way to relate linearly the measured 
torques or forces to the dynamic parameters, which can be estimated by any 
standard identification technique (203. In this section one of the identification 
schemes based on recursive least squares techniques is discussed. 

Consider the NE equations (A.l) and (A .2) derived in Appendix A. These 
equations are clearly nonlinear with respect to the dynamic parameters of each 
link i. But when they are rewritten by expressing the moment of inertia I; about 
the joint origin i instead of about the center of mass Cj, these equations become 
linear with respect to a set of equivalent dynamic parameters. For example, 
consider the two link planar manipulator model discussed in Appendix A. In eqns. 
(A .3), (A .4), (A .5) the terms ( foi- fia ) , fia, and ( Noj-N la h represent the 

net force and net torque due to the motion of the individual links i and 2 alone, 
respectively, which can be measured by means of force- torque sensors. Then eqns. 
(A.l) to (A .5 ) are rewritten to obtain the following equations, 


f 12 = ma Vc 2 - g 

( 4.1 ) 

^oi“ "fia = Vci" »ig 

< 4.2 ) 

Nia = la Wa + X fi2 

( 4.3 ) 

Noi -Nia = IiWi + ricgX foi + X fia 

( 4.4 ) 

Using the fact that for the two degree of freedom planar manipulator 

Nia = the following equations are defined. 

Npi = Ui and 

■fast — iWa Vca " •**2 9 

C 4.5 ) 

f 11 = mi vci - iHi g 

( 4.7 ) 


where f jt and fa* are the forces at joint 1 and 2 due to the motion of the 
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individual joints alone. 


Let a set of auxilary parameters be defined as 
h' = U + Icf 
k' = I 2 + ®2 lc| 

ICi' = ffli Ici < 4.8 ) 

Ica' = iHg Icji 

*Then rewriting the dynamic equations of the two degree of freedom planar robot 
manipulator can be rewritten using the above relations, 

= I ai3 Ii' + Iqi + q23 I 2 ' + t + g cos(qj^> 3 nis 4- IC 2 ' E -2 Ij, cc® (qg) qx 
- sin (q 2 > qg - 2 li sin (qg) + +9 cds( qj + qg) 3 

+ E g cos (qi) 3 Ic/ ( 4.9 ) 

2 

Uz = f qi +^) V + IC 2 ' C cos {qjj) qj. + 2 sin(q 2 > di +9 cos ( q^ + qj, ) 3 


faa 


2 

E -qi sin (qj.) - qj. cos (qj) 3 Ic/ 
2 

E Cl cos <qi) - di 3 Icj.' -g[iii23 


The elements f u. , i = 1, 2 of the vector f n are given by 
f - C li cos <qi) q'i*+ li sin ictj} q^ 3 m 2 - E cos (qj-Hfe) (di + da 3® 
+ sin teit-fq 2 ) ^i. + 02 ) 3 Icz' 

f 112 = - E sin (qji) -cos (qi) 0^+ g 3 in2 - E sin <qi+q 23 tdi + da)* 

- cos Cqj + qa) (di + da) 3 Ica' 


The above equations are linear with respect to the selected equivalent 
auxilary dynamic parameters namely, I*', V , Ici' , Ica', and the link masses nia and 
iBj. The above equations are confined in a vector matrix form to obtain the 
following equations. 


M = Y ♦ 


< 4.10 ) 



mi 

Ujl ICt' 

fii h' 

, § = 

Us IHs 

f22 ICs' 

W 

and Y is a C 6 x 6 ) matrix whose elements ( i = i...6 , j = i...6 ) are given 
by, 

«12 = 3 COS( Qi) 

HiS = 

2 

ai4 = ll di + li 3 cos ( Qi) 

2 

315 = 2 li COS ( ds ) di + lx COS ( Qs > ds -lisin ( os) Os -2 Ixsin ( Os ) Oi % 

+ g cos (qi +02 ) 

yifi = di + d2 

2 

= - C cos ( Ox ) 02 + dx sin ( ox > 3 
931 = 9 

332 = cos (Oi) Ox -di Ox sin <qi ) 

345 = ll cos <02) Ox + 3 cos (Oi +tfe > + lx sin < 02 ) dx* 

34£ = di + ^ 

z 

354 = - t ll sin ( Oi ) Ox + lx cos < Ox > Ox 3 

355 = - ll cos < 01 + 02 ) < Ox + ^ )^ - sin < Ox + 02 ) % 

364 = ll cos < Oi > Oi + 3 - ll sin < oi ) Ox* 

36s = cos ( Ox + 02 ) ^ + 00 s <Oi + O 2 ) dx - sin < Ox + 02 ) < Ox + Cfe )® 

The rest of the elements of 3 are zeros. 

The elements of Y can be calculated at each sampling instant from the 
measurements of joint angular positions, velocities and accelerations. Generally, 
the acceleration measurements are derived from the velocity measurements. So the 
acceleration measurements can be obtained by differentiating the velocity 
measurements. The elements of M matrix can be derived from the torque/force 


where U = 
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sensors So from eq. (4.10 ) a Recursive least squares scheme can be formulated 
to identify the # vector at each sampling instant, from the measurements of joint 
positions, velocities and forces/ torques. This algorithm is given below. 

AlgoritNn : 

Input data : f(0) , P(0) and X 

Parameter update at each instant k is computed as, 

I ftc) = I ( k-1 ) + P(Jf) Y^(k> I Wk> - Y<k-D | <k-D 3 ( 4.11 ) 

with 

P<k> = 1/X P(k-D Y^ft:-D C X I + Y<k-D PCk-D Y^<k-D 3 Y(k-1> P«f-1> 3 < 4.12 ) 

where 0 ^ X 1 is a forgetting factor which provides an exponential weighting of 
the past data in the estimation algorithm and by which the algorithm allows a drift 
of parameters. 

P is a symmetric positive definite matrix of appropriate dimension. 


4.2 Control scheme ; 

Por the purpose of the formation of the control law the state equation 
representing the robot dynamics, i e., eq. (2.4 ) is modified as 

X = f (x,u,t) < 4.13 > 

A 

where f ( ) is the vector function f ( > evaluated at the identified dynamic 
parameters. With the above modification the linearized perturbation equations 
(2.26) , (227) and (2.28 ) can be modified as 

3x = Vx fid ^ Ct) + Vu Hd SlKO < 4.14 ) 

» Mt) 8x0.) -*■ mt) StKt) 

where Vxfld and Vu "^d the jacobian matrices of fCxCt)* uCt) ) evaluated at 
Xj^t) (the desired state vector at time instant t ) and Uj^t) ( the desired torque 
vector at time instant t ) obtained form the NE equations with the identified 


dynamic parameters. i.e.. 
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Ujj-D(^)gy + A(^,^)g^ + ^g^J 

( 4.15 > 

S^t) = X (t) - Xjj (t> 

( 4.16 ) 

8y(t) = u (t) - ^ (t) 

( 4.17 > 


The optimal control law which minimizes the following performance index 
oo 

J = ^ r 5x a) + su%) e suct) ) dt ( 4.18 > 

0 

where F = F^ S; 0 and 6 = 6*^ >0 is given by 

uCl) = - e"^ T ^ ^ ( 4.19 > 

where T = T > 0 is the solution of the algebraic Ricatti equation, 

a'^'t + ta + F - TB = 0 <450> 


The above formulation is based on the assumption that the state 

measurements are noise free. In the case of noisy measurements the modified 

performance index becomes 
ao 


1 



(5xT(t) F Sx (t) + Sum) e 5u(t) ) dt 


1 


i A.Zl ) 


and the optimal control law which minimizes the above performance index is given 
by 


u (t) = - B^ T Sx 


( 4J22 > 


where Sx = x - 

and X is the estimated state vector from the noisy measurements of x obtained 
from either eq. (2.15) or eq. (2.16 ) 

With the identifier introduced into the system the modified control scheme is 
shown in fig (4.1). First the noisy measurements are filtered using either of the 
two state estimation schemes and the filtered measurements are fed to the 
identifier. The identified dynamic parameters are used to update the nominal 
control (feedforward) component using NE equations and simultaneously to 

A A 

generate the perturbation matrices A (t) and B(t). 
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A step fay step algorithm for the total control scheme is given below. 
Algorithm : 

Initialization ■- 

Intialize matrices Q, R, and P(0> and initial state x <0). Find 
suitable 0 by following the guidelines given in eq. (3.11 ). 

For CGEKF, choose L matrix by trial and error to give maximum 
convergence. 

Initialize #(0), P(0) of the identifier by using the a priori 

information about the dynamic parameters of the manipulator. 
Initialize the F and (& matrices of the controller. 

StepL : 

Tune the system parameter of the model x= f<£,T,t> by 
f(x,y,t) = fCx,u,t)- 06 
Step 2 : 

Use the EKF or CGEKF algorithm to find the estimates of position 
and velocity using the system and measurement model as given 
here. 

X = u ,t ) + ^(1) 

li{k)= t]^3^+ u^^ 

and giving output t x f 3 ttjj) g ( ) 3 

Differentiate the estimated velocity vector to obtain the 
acceleration vector g. 

Step 3 : 

Run the identifier to find the estimates of dynamic parameters 
using the torque /force measurements, the filtered measurements 
g, It and the derived acceleration measurements g Find the 
nominal torque from the following relation 

= D(gd,gtj)^ + H (gj,gj)gd + 9 i > 

where 3d> ^ ^ desired angular position , velocity 
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and acceleration of the joints respectively and D, H and g are as 
defined in eq (2 i) 

Step 4 : 

Evaluate the jacobian matrices A(t) and B<t) given by eq. {.226 ) and 
solve the Ricatti equation (2.32) . The perturbation torque is 
given by, 

fiu (t) = - 0 ^ B^(t> T6x (t) , where T is the solution of the Ricatti 
equation. The control input to the system is then 
u (t> = (t) + S utt). 

13 Numerical Examples 

To check the performance of the identification scheme and the control 
scheme discussed in this Chpater experiments are conducted on the simulated two 
degree of freedom planar manipulator which are discussed below. 

13.1 Simulation Experiment for Identification Scheme : 

The identification scheme discussed is implemented on the simulated two 
degree of freedom manipulator. To make the simulation realistic the 
measurements are corrupted with noise. It is found that the algorithm is very 
robust and has very good convergence property. It is observed that the above 
algorithm has high sensitivity to the noise in the acceleration measurements. Table 
(4.1) shows the simulation parameters Fig . (4.6) shows the simulation results. 


13.2 ; Simulation Experiment for the Overall Control Scheme -. 

The performance of the above control algorithm is studied by conducting 
tests on the simulated two degree of freedom planar manipulator. The simulation 
tests are carried out separately with the two state estimators namely, EKF and 
CGEKF. The actual robot effect is simulated by using the actual values of dynamic 
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parameters in the during the forward dynamics calculation. Fig (4.2) and (4 3) show 
the tracking error plots for the trajectories i and 2 respectively, with EKF as 
the state estimator, while fig (4.4) and fig (4.5) are the respective plots for the 
control scheme with CGEKF as the state estimator Table (4.2 ) shows the list of all 
the parameters used in these simulation studies By comparing these results with 
those obtained in Chapter 3 it can be observed that with the identifier in the 
closed loop the performance of the system has improved significantly. The maximum 
tracking errors and the variances of the tracking errors for the two trajectories 
with EKF and with CGEKF are shown in table (4 3) and (4.4) respectively. It can be 
observed form these results that tracking errors with CGEKF are slightly higher 
than the tracking errors for the control scheme with EKF From these results it 
can be inferred that with a slight degradation of performance, CGEKF can be used 
for the state estimation which reduces the computation time drastically. 


Table 4.1 


P { 0 ) = 100 1 , X = 0.975 
where I is a 6X 6 identity matrix 
Initial V'alues of Dynamic P^ameiers 
= 14 

ICi = 0.46 
Ii = 3.75 
fiij2 “ 0 .9 
Ica = 0.175 
h = 0.533 

variance of noise in position measurements = 0.001 
variance of noise in velocity measurements = 0.05 
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Table A2 

Besian Parameters of the Identifier: i Refer Table 4.i ) 
Design parameters of the iraoking controller ■ 

Q = diag (iOOO, 1000, 1000, 1000 ) 

R = diag (1, 1) 

Design parameters of the EKF : 

R = diagC 0.001, 0 001, 0.05, 0.05 ) 

Q = diag( 0.0025, 0.0025, 0 050, 0 050 ) 

P<0>= R 

0 = diagC 0.9, 1.0, 1.1, 1.3 ) 

Design parameters of the CGEKF : 

R = diag( 0.001, 0 001, 0.05, 0.05 ) 

L = diag ( 0.01475, 0.01475, 0.225, 0 225 ) 


Table 4.3 




Haxiinuni TracAcing Error 

Haxiimjnfi Trac^dng Error 

rrajectcwHj 


with EKF 

' with CC3EKF 


joint 

position 

velocity 

position 

velcsity 



(rad) 

<rad/s) 

iradi 

(rad/s) 

Semi- 

1 

0.00318 

0.02729 

0 06248 

0.05252 

Circular 

2 

0.01394 

0.07957 

0.02007 

0.17122 

Elliptical 

1 

0 01429 ' 

0.05128 

0.01416 

0.05291 


2 

0.01070 

0.07410 

0.01640 

0.13528 



















Table 4.4 


Trajectory 


joint 


Circular 2 


Elliptical 1 
2 


Variance of Tracking Errcw' 

with EKF 

Variance of Tracking Error 

with CGEKF 

position 

<ra<D 

velCHcity 

irad/s) 

Dcjsiticn 

(rad) 

v^Dcity 

ir-ad/s) 

0.00029 

0.00028 

O.OOiiO 

0.00289 

0.00075 

0.00025 

0.00248 

0.00473 

0.00028 

0.00052 

0.00106 

0.00580 

0.00013 

0.00491 

0.00129 

0.00602 
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CHAPTER 5 


Robust 

Computed Torque Control Schemes 


In this Chapter two control schemes are discussed which incorporate a 
special form of feedforward term for full dynamics compensation These control 
schemes differ from the control schemes discussed in the earlier chapters in the 
sense that they use the full nonlinear feedback compensation unlike the control 
schemes discussed previously which use the linearized perturbation model for 
feedback compensation . One of these schemes is essentially a modification of 
scheme til] and the other is proposed by Majee [153. The control schemes £153 and 
[113 are briefly dealt. The performance of these control schemes in presence of 
measurement noise is studied by conducting simulation tests on the two degree of 
freedom planar manipulator. 

5.1. Controller With Sliding Mode -. 

Cori 5 ider the manipulator dynamic equation (Z.i): It is to be noted that in the 
dynamic equation the two matrices D and H are not independent of each other Ciil. 
Given a proper representation of H, the matrix C D - 2H ) is skew-symmetric. Using 
this fact the following equation can be derived. 

D = H + since is D symmetric . < 5.1 > 

Taking into account the influence caused by unknown variation of the system 
parameters, the dynamic equation of the robot manipulator can be written as, 

<D+ aD> g + (H + AH) g + (g+’Ag) = u < 52 ) 

where modeling errors are represented by AD, and Ag . 
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Deoendi™ on these parameters the foUo«*na control law is soggesUd [28 1. 
u = Wa)[ar-r(Aa-er3r-td.l^H<g,g^,a.^ 9 ( a ) - Kp s (53) 

where the vector a- and Jr are called reference velooitg and reference 

acceleration respectively, and are defined as, 

gr = gd “ ^ { 5 .4 ) 

^r = Sid - < 5.5 ) 

for a constant positive definite matrix C. The position and velocity tracking 
errors are defined as. 


§ = a - Qd ( 5.6 ) 

g = g - ^ 

The vector s is a combination of position and velocity tracking errors and can be 
taken as a measure of tracking accuracy. This is defined as 

s = g + C fi , ( 5.7 ) 

= g - a- 


Defining s as the sliding surface, to let the tracking error to zero the 
- T 

condition ss <0 should be satisfied. This ensures that the system states 
continue on the surface s=0. Under the assumption that sliding mode starts and 
continues on the surface s=0, V g and V a the value of s can also be 
approximated to zero for all g and Thus, from (5.7 ), 

g ar ^ ( 5f8 ) 

g = ^ ( 5.9 > 

Eqns. (5,5 ) and (5.9 ) are combined to obtain 

D ( e +C g ) = 0 C ( A -AD' )gr + (r-AH')g + (d-Ag')3 = u ( 5.10 ) 

Substituting the eq. (5. 7 ) in eq. ( 5.11) 

s = (A-AD')ft-+(r-AH')^ + Cd-Ag') ( 5.11 ) 

where aIT = IT^AD , Aff * D-^aH and AgT = D“^aD 

Now, for sliding mode to occur SjSj < 0 for i = i . 2 .n. By using eq. ( 5.11) 

the following relationship can be obtained. 
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Aij -A D' ) c^j s j + ^ ' 

^'=1 

The goal is to find Ay and rjj 


< H jj - aH' ) S{ + < dj - A^ ) Sj < O C 5.12) 

and d; so that the above ineauality is always valid. 


They are chosen as follows, 
A\i = -A1 y sign ( s; ) 
Fy = -A2y sign (Sj ) 
d-, = - l?li sign ( S; ) 


( 5.13 ) 
C 5.14 > 
( 5.15 ) 


where Al and a 2 are positive definite matrices and 7}i is a vector with all 
positive entries as defined below. 

Al = maximum variation in D ' matrix and is known. 
a 2 = maximum variation in l-T matrix and is known . 

= maximum variation in vector and is known. 

For checking the stability of the control scheme, consider the following Lyapunov 
function. 

V(t) = l/2s^Ds (5.16) 

Differentiating Vtt) with respect to time , 

V(l) = 1/2 s^D s + s^ D 5 

= 1/2 s^D s + s^D [ g - ^3 
= s^/2 LCD -2H>s + 2 (u - g -I^> 3 

Cconsidering the conservation of energy in the manipulator, the following 
relationship can be derived CilX 

1/2 d/dt [ g^ D g 3 = g ( u -g ) 

where the left hand side is the derivative of the manipulator's kinetic energy and 
the right hand side represents the power input from the actuators . 

Differntiating the right-hand side explictly 
g ( u -g ) = g'D a + g D g 
and expanding the term Dg by using eq. (2 1) , 
g^< D- 2 H ) g =0 
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and from this it is clearly observed that 
s'‘'( D- 2H ) s =0 


Hence, 

Wt) = s E u -H gp -g -I^p 3 ( 5.17 ) 

Now replacing u with the control law given in eq. C 4 ) , then eq. ( 14 ) becomes, 
V(t) =s^ [ ( A -Aiy > gr + ( r - AH' ) ^ + ( d- Ag ) 3 -s'T Kjj s 

From eqns.(5.i3), (5.14) and (5.15) it is evident that the first term within the 
square brackets is always negative • for any s except s=0 . and also since Kq is 
positive definite, Wt) is always negative definite. Hence the above formulation 
leads to the global asymptotic convergence of the tracking error Now due to the 
presence of sign(s) term in the control signal there will be excessive control 

activity even in the case of small error. To cope with this problem a function f(s> 

instead of sign(s) defined as below, is used. 

/ 1 if Si > ^ 

fEsj) = ^ s/^ if ^ < Si < ^ { 5.18 > 

\ -I if Si i ^ 

# is called the control bandwidth and this is totally dependent on designer's 
choice. So finally, the control law becomes, 

u = IXg) E gr+ ( + ri^ + d)l + H^,^)a'+ gEgl-K^s ( 5.19 ) 

with 


A;. = -Al y f<Si > 
Fy = -A 2 ;j f(Sj C|pj ) 
d; = - J?l; ffei ) 


( 5.20 ) 
( 5.21 ) 
( 52Z ) 


Using this formulation Majee C153 designed a control scheme in which the 
noisy feedback measurements are filtered using the Extended Kalman Filter 

theory. 

From the simulation studies of the above control scheme Majee remarked 
that under high amount of modeling imperfection, the sliding mode control gives 
rise to control chattering which may excite the high frequency unmodelled 
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dynamics. In order to overcome this difficulty a control scheme in which the 
robot dynamic parameters are identified as a part of the control objective i.e., to 
force the error between the actual and desired state trajectories to zero is 
studied below. Fig (5.1) shows the block diagram of the overall control scheme in 
corporating the sliding mode control and the Extended Kalman Filter to 
disambiguate the noisy feedback measurements 


6-2 Adaptive Control Scheme : 

This scheme exploits the fact that the robot dynamic equation can be 
recasted such a way that it is linearly related to a particular set of equivalent 
robot dynamic parameters. The dynamic equation of the robot model, eq <2. i) can 


be rewritten as shown below. 

/ cr \ 

iXg>a + H(§.9)a + 9<a>=W(g.9,g)B 

where p is m dimensional vector containing the unknown and recast dynamic 
parameters, and W is nx m matrix independent of the vector p. 

Using the eouelion Slotine and Li til] suggested the following oontrol law, 

u e B<g> c ^ ^ 

r i 5.^ ) 

0 = - r w‘s 

where r is a constant positive definite matrix 

To oroua the globid asgmototio stabilitg of the above control law the following 
Lyapunov function is considered. 

T w,T -i ~ , < 5-26 ) 

v<« = i/2 cs'd s + p r P 3 

Diffemtiating VO) with respect to time and proceeding in similar lines to that of 
the derivation for the sliding mode controller the following edualion can be 

obtained. 

^ s^( 

= sTiwp-Kd 53+ p‘r"p 

Substituting ed. (5 25 > in the above it can be shown that 


WO = sT( D ». f H flr -9 - Koe > + o 0 

"j* _ - wfT-r**"!* SC 
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V(t) = -s^KjjS i 0 i 527 y 

which shows the global asymptotic convergence o-f the controller. 

In order to take into account the noisy feedback measurements the EKF 
algorithm discussed in the previous chapters is used. The overall block diagram of 
the control scheme is shown in fig (5.2 ). 

5.3 Simulation Experiment 

The performance of these control scheme are studied with the help of the 
simulated two degree of freedom planar manipulator The manipulator dynamic 
equation (2.1 ) is parameterized as given below. 

Ui = 1/ Qi + V ( Qi + da J t Ijf Qi + 3 li COS ( qi) 3 

+ Ica' [21iCos (Oa) Qi + li Oz cftssCcfe) - li sin ( 02 ) tfe -2 li sin (q 2 > aiOz+gcosCdi+qa ) 3 
+ Ici' t2 qi cos(qi) + li sin <q 2 > + 3 cost Oi ) 3 

Ua = I 2 ' ( qi + ^ ) + Icz' C 3.i qt cos (qfe) + li sin(q 2 > eiz ^ + 3 ^ di +02 3 

where 

li' = li + IHi ICi® 

I 2 ' as I 2 -I- nia lOjp 

ICj' as milCj. 

Ica'aa malca ar'e the equivalent dynamic parameters. With this formulation the 
equivalent parameter vector p is given by 

pT _ j I 2 ' m 2 lea' Idi' 3 ^and the elements i=i.. 5, 3=1.. 2 of the W matrix are 

given by 
Mil = Qi 
W12 = < di + ^ 3 

Hsa = 1 / + 3 

„i^ - 2 005 (ci2>qi + li^ °oa<d23 - U sin (cfe) da® - 2 lisin (da) di^ + 3 oosCdi-Ki2> 

Mi s = 3 cos (ai3 
W22 = di+ ^ 
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W24 = lx Qx cosCqa) + lisin + g cos { q 4 + q^) 

The rest of the elements of U are zero. 

The matrix r is chosen to be an Identity matrix. 

Regarding the state estinriator, all the arguments made in the previous 
chapters are valid for this case also. The nominal trajectory in this case is 
assumed to be a circular path having constant acceleration, uniform velocity and 
constant deceleration regions respectively The trajectory is generated using the 
procedure described in chapter 2. The duration is of this trajectory is assumed 
to be 4 sec. The nominal joint trajectories are shown in fig. (5.3) The simulation 
experiment is performed at different load conditions with a maximum load of 4 Kg. 
attached to the second link. The performance this control scheme is compared 
with that of the sliding mode control scheme proposed in Ci53. In table (5.i) the 
values of the parameters used in this simulation studies are shown. Fig (5.4) and 
fig. (5.5) show the tracking error plots for full load with identifier and with 
sliding model control respectively. Similarly, fig. (5.6), (5.7), (5.8) and (5.3) show the 
respective plots for half load and noload conditions. Table ( 5.2) and (5.3) show 
the maximum tracking errors and the variance of the tracking errors for the two 
control schemes discussed in this chapter for the three different loading 
conditions. It can be observed from these results^ both the schemes result in 
almost the same performance and the performance of the adaptive control scheme 
is marginally better than that of the nonadaptive scheme in the case of large 
parametric variations. 
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Table 5.1 

Design Parameters of the tracking controUer -• 

Kp = diag ( 400, 300 ) 

C = diag ( 8, 8 ) 

For sliding Mode control : 

# = O.Oi unit 

Al, A2 and Hi '■ calculated from the nominal dynamic Parameters given in Table C3.1) 
Parameters of the Filter : 

R = diag ( 0.001 , 0.001, 0.05, 0.05 ) 

Q = diag ( 0.003, 0.003, 0.005, 0.005 ) 

P<D) = R 


^ = diag ( 0.8, 0.9, 1.0, 1.3 ) 
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Conclusions 


The performance of the control schemes designed in this thesis is 
evaluated and the practical implementation details of the actual control scheme 
are also presented. Finally scope for further work in this direction is suggested 
and the thesis concludes with the summary of ideas presented. 

6.1 Comparison of results : 

The results obtained in Chapter 4 and Chapter 5 can be compared. From the 
Table. (4.3) and (5.2) it can be observed that the tracking errors with LQ controller 
are smaller by one order. But these results can not be compared in absolute 
numerical terms, since it is expected that the simulation programs were not 
identical in terms of nominal trajectories. This can be due to the fact that for the 
nonlinear controller the control law is based on fixed PD feedback whose gains 
are chosen by intuition where as in the LQ case the gain matrix is calculated 
through minimizing the quadratic performance index. 

The control scheme based on linearized perturbation equation has the 
disadvantage that in the formation of perturbation equations is is assumed that 
the tracking errors are small and hence the perturbation equations are valid 
only in the neighborhood of the nominal operating point. In the case of any large 
disturbance the control scheme may fail, the calculation of Jacobian matrices A 
and B , and the solving Ricatti equation on line restricts the application of this 
control scheme. The next section deals with this problem. 
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^■2 Implementational Aspects : 

The various computational aspects regarding the actual implementation of 
the control scheme are dealt here. The control scheme essentially consists of a 
feedforward term, feedback term, identifier to estimate the dynamic parameters and 
a filter to disambiguate the feedback measurements. The overall control scheme is 

computationally cumbersome and it needs some modifications to physically 
implement the scheme which are discussed below. 

It can be observed that the feedback and feedforward terms are totally 
independent of each other. So two separate microprocessors can be used to 
calculate these terms simultaneously. 

In practice the parameters of the robot can be measured or estimated 
before hand. Since the load is usually connected to the last link, only the 
parameters of the load are unknown. So instead of identifying the whole set of 
dynamic parameters, only load parameters can be identified online reducing the 
computational burden. 

The matrices A, B may be updated at a low rate while a high rate can be 
used to update the terms g, ^ q. Further the identification can also be 
performed at low rates. 

Instead of solving the Ricatti equation online one step ahead optimal 
control law can be used which does not require the solution of the Ricatti 


equation. 
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6.3 Scopo for Further Work : 

The work done in this thesis is purely simulation and no theoretical proofs 
are given regarding the convergence properties of the modified EKF algorithm and 
the the robustness properties of the control scheme and the identification scheme. 
So studies in this direction may give some interesting results. 

The control scheme discussed in applicable only for the joint position 
control and many of the modern day robots are required to perform complicated 
tasks which require control of not only the joints positions, velocities but also 
the force exerted by the manipulator on the environment. The feasibility of the 
present control scheme for joint position and force control is to be investigated. 

6.4 Conclusions: 

It has been shown with the aid of illustrative examples that the LQ control 
scheme can be successfully utilized to stabilize and control a robot manipulator. 
The control of two degree of freedom planar manipulator is presented. This may be 
easily extended to consider multiple joint control. 

Two different nominal trajectories are chosen for simulation studies to 
determine the capability of the controller to cope with different operating points. 
In both the cases the tracking errors are small demonstrating the versatility of 
the controller. 

Models of Columb friction can also be included in the dynamic model of the 
robot and the corresponding coefficients can be identified. 



APPENDIX A 


A.l Newton-Euler Formulation of Equations of Motion : 



Consider the free body diagram of a robot ;Shown in fig (Ai) 

Let fj_i^ be defined as the force exerted by the link on the i^^ link, m-, is 

the mass of the link i , is the linear velocity of the centroid Q of link i w r. t 
a fixed coordinate system, (see fig ) The balance of linear forces then yields, 
fj-i, i - f !, j+i +"»; 9 -n»i vg = 0 i =1 ../» < A .1 ) 

Where all vectors are defined w.r.t. .9 base coordinate frame and there are n links. 

Similarly if is defined as the movement applied to link i by link (i -1 ) , 

IS the position vector from point CVi to 0; in figCA.i) , W; is the angular 
velocity vector and I; is the centroidal moment of inertia of link i , then the 
balance of moments can be derived as 
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Ni_i^ -Ni+ij - r;_i C; X + Tj C; X fj+i^ "li - «^X ( Ij W; > = 0 { A. 2 ) 

i = i, .... n 

Eqns. (A.l)and (A 2) can be modified so that they are expressed in terms of 
input variables which can be measured . Choosing the joint torque u as the input 

and joint displacements 3 = C qj, Oa OrJ ’’ as the outputs that locate the 

whole arm we derive the dynamic equations which correspond to a two degree of 
freedom planar manipulator C fig(2.1) ] with two individual links are given below. 
The equations are interms of joint torques uj and Ua and joint displacements qj 
and qa of the two links. 

Eqns. ( A.i) and (A 2 ) for link 1 and link 2 respectively are, 


^^0,1 "^^1,2 + 11*19 -mi Vci =0 < A.3 ) 

No,i~ Oi^ = Cl X fi^ -1; Wj = 0 ( A.4 ) 

fi,a + ***2 9 "Hta Vcfl == 0 ^ A.5 ) 

-ri CaX fi^ -I 2 wa = 0 ( A.6 ) 

For the planar maipulator have N 0,1 = Ui and = Ua- Also, the 


angular velocities Wi and Ma can be expressed as Wi = qi and Ma = qa+ cii 
and the angular velocities Vci and Vca can be written as 

Ici q'lSin Pj 
Icidicceai 

i lisinQi +lcf2 sin(qi+q2>3 dli -IcaSMnCqi+qj^ ^ 

— 

£ licosqi+lca cosCqi+qal 5 Pi- Icacosfcii+qajqa 

( AB } 

Substituting eqns.( A. 7) and (A .8) along with their time derivatives into eqns.( A.5) 



( A.7 ) 



and (A .6), we obtain the closed form dynamic aquations in terms of Qi and 02 
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IXa)g + H(ga> + g(g> = u 


Where y = 


ui 

U2 


a = 


di 

da 


D(a) 


Dii Di2 

Dzi D 22 


H ( a ,a> = 


Hii 


and 


g( a > = 


3ii<d) i 
S2i(o) 


2 2 2 

Dii = milCi+ m2< li +IC 2 + 2 lilc2Cos<d2) + I 2 +I 2 

2 

D^2 = rB2fllC2COS<q2)-»-m2lC2 + I2 

I>2i = Di2 

D 22 

2 

- m2lilC2Sin(q2) 02 -2 m2lc2liSin<q2)ciiq2 
2 

H2i= m2lilc2 sin(q2)d2 

9ii= iflilCig cos(qt)+m2g ( lc2Cos(qi+q2)+liCos(qi)) 
321= m2lc2g cos(qi+q2> 


( AS ) 



APPEJiDIX B 


Bi Derivation of the fj and ^2 : 

Consider the manipulator dynamic equation ( 2.2 ). The determinant o-f the 
D(g } matrix representing the manipulator inertia is given by 
iDl = iitjt lC|^ Icg + ( mi ing lcilc2 + m 2 IC 2 Ii + mi Ici I 2 + Ii I 2 

- <m2lilc2)* 005 ^( 02 ) < B.i ) 

Now 3 =s D~^ (g)tu-H(g,g)-g(q)3 < B.2 ) 

>1 

From eq. ( B.2 ) it is clear that the fi and ^2 sf'e given by the first and second 
rows of the matrix D~^ (g )E u-H< 3 ,g)-g(q) 3 . 

Using eqns ( B.I ), ( B. 2 ) and ( 2 . ) we get 

f 1 = A C Cl Ui - U2 + 02 COS ( Qi > + 03 COS ( Q2 > COS <01 + 02 ^ 

|D| 2 2 

-f C4 U2 COS ( 02) + C5 sin ( 02 } ( % +2 3102 ) + cs sin < 02 > di 

+ Cfi /2 sin ( 202 > 3 

_ flu 
iDl 

where 

Cl * m 2 lC2 + h 

C 2 = "CiB ( milci + m 2 li 1 

jH St 

Cq = m 2 ll lC2g 
C 4 = -m 2 lilca 
Cs = -Cl C 4 


Similarly the expression for f 2 is given by 


J:. 

IDl 


E -Cl Ui + 04 Ui COS {q2)-*-07U2-C2 CCS { Oi) + Og /2 sin ( 2 02 1 

9 St St 

( 02 + 2 OiCte + di > + Cs sin ( Gte +2 Oi^ > + cg cos < Oi > cfos <02 > 

2 

COS ( Oi + Q® )C 0 S( 02) + Cg COS ( Oi + ^ i Oj. 


*^2 


82 


where 


2 2 2 
lCj[ 4* 1112 ^ ^2 +11^ + 12 


Ca = raa li 1 C 2 g ( iHi Ici + mali ) 

Ca = nta Icfa g ( Ci - c? ) 

Crj^Q ” “^7 Icfa 

B.2 Expressions for the Jacobian Matrix Elements *. 

= rli r "°2 sin C Qi ) - C 3 cce { 03 ) sin ( Qi + Oa ) 3 

aqi iDl 

^ c f ' IDI - flu tDl' 3 


^ IDI® 


where fiu and iDl' are given by 

f ' - = Ca E -sin (q®) cfos ( qi + qz) - sin ( qi + qz ) ccs ( Qz > 

9 ci 2 2 . . .2 

+ c* Uzsin ( qz) + Os cos (Oz) < ^ +2 qiOz + Qi ) 
2 

+ Cs CM3S ( 2qz ) cii 3 
iDl* _ | 5 l] = ( injj Iczli >* sin (Zqz) 

aqz 

?!fi = -i-, C 2 Cs sin ( qz )< ai + Cte ) + Cs sin < 2 q 2 ) cii 3 

aqi IDI 

= rh C 2 Cssin ( qz) < Qi + 3 

002 il^l 

^ - X E Cs sin C qi ) - Cg sin ( di ) cos ( qz ) - Cg sin ( qi + Oz ) 

9cii lOl 

+ C 3 cos ( Oz ) sin (Qi + dz ) 3 

A C WIDI -f2ulDl'3 
302 IDl® 

where fau' is given by 




^gu - C { C 4 U 1 + 2 C 4 UZ - Cg cos ( qi) ) sin C Oz > - Cs cos < 2qz > ^ 

^ (^* + 2 qi^+ q/) -Casin<qi + q 2 ) + CioCOs(qz)cii 

+ Cs sin ( Zdz + Oi) -C 5 COS ( qz)C Oz +2 aiOfe > 3 


Sfg _ X c - c« Sin ( 2qz ) ( 2 qi+ qz ) + 2 Cg sin ( qz ) 02 ) + 2 Cia sin ( Oz) di 

aqi " IDI 

3^2 _ X c ( -Cfi sin ( 2qz ) + 2 Cgsin C Oz) < qi+^ 3 
ditz ~ |D| 


8 ui " IDI 
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§fi = J. 
au2 iDi 

3f2 _ A 

3ui “ iD| 

§fi - A 

aj2 “ iD| 


l -Ci + C4 cos ( 02 ) 3 
C -Ci + C4 cos ( 02 ) 3 
E C7 - 2 C4 cos ( Oa ) 3 


K(5H-RoB. 



